#ifndef LIDAR_PERPROCESS_AVERAGE_GROUND_SEG_H
#define LIDAR_PERPROCESS_AVERAGE_GROUND_SEG_H
#include "../lidar_preprocess.h"
// #include "common/log.h"
#include <cmath>
#include <iomanip>
#include <math.h>
#include <pcl/common/io.h>
#include <vector>
namespace perception
{
    namespace lidar_preprocess
    {
        struct cord
        {
            float x;
            float y;
        };
        struct grid_obj
        {
            float x;
            float y;
            float zmin;
            float zmax;
            int point_num;
            int vaild;
            int idx;
            bool is_seed;
            bool is_used;
            void reset()
            {
                x = 100;
                y = 100;
                point_num = 0;
                vaild = 0;
                zmin = 3;
                zmax = -3;
                idx = -1;
                is_seed = false;
                is_used = false;
            }
        };
        template <typename T1, typename T2, typename T3>
        class AverageGroundSeg : public LidarPreprocess<T1, T2, T3>
        // class AverageGroundSeg 
        {
        public:
            void Process(typename pcl::PointCloud<T1>::Ptr &data_in, typename pcl::PointCloud<T2>::Ptr &data_out, T3 &config);
            void Process2(typename pcl::PointCloud<T1>::Ptr &data_in, typename pcl::PointCloud<T2>::Ptr &no_ground_points, typename pcl::PointCloud<T2>::Ptr &ground_points, T3 &config);
            // AverageGroundSeg(T3 &config)
            AverageGroundSeg() {}
            ~AverageGroundSeg() {}

        private:
            int GetGridZmin(const typename pcl::PointCloud<T1>::Ptr &data_in,
                            std::vector<std::vector<grid_obj>> &grid_preprocess, int &row, int &col,
                            T3 &config);
            int GetGroundPoints(const typename pcl::PointCloud<T1>::Ptr &data_in,
                                std::vector<std::vector<grid_obj>> &grid_preprocess,
                                typename pcl::PointCloud<T2>::Ptr &ground_points,
                                typename pcl::PointCloud<T2>::Ptr &no_ground_points,
                                int &row, int &col,
                                T3 &config);
             int GetGroundPointsOld(const typename pcl::PointCloud<T1>::Ptr &data_in,
                                                          std::vector<std::vector<grid_obj>> &grid_preprocess_,
                                                          typename pcl::PointCloud<T2>::Ptr &ground_points,
                                                          typename pcl::PointCloud<T2>::Ptr &no_ground_points,
                                                          int &row, int &col,
                                                          T3 &config);
            void ComputeGoundSeeds(std::vector<std::vector<grid_obj>> &grid_preprocess,
                                   std::vector<cord> &cord_size, int &row, int &col, float &ground_height, float &ground_height_eps);
            void ComputeGound(std::vector<std::vector<grid_obj>> &grid_preprocess, std::vector<cord> &cord_size,
                              int &row, int &col, float search_scope, float &grid_gradient_eps);
            void ComputeGradient(std::vector<std::vector<grid_obj>> &grid_preprocess, std::vector<cord> &cord_size,
                                 int &row, int &col, float search_scope, float &grid_gradient_eps, int x, int y);
            void Grid(std::vector<std::vector<grid_obj>> &grid_preprocess, int &row, int &x, int &y, float &z, T3 &config);
            inline bool IsUsed(std::vector<std::vector<grid_obj>> &grid_preprocess, int x, int y);
            inline bool IsSeed(std::vector<std::vector<grid_obj>> &grid_preprocess, int x, int y);
        };
    }
}
#endif